COMMENT ⊗   VALID 00007 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	ENTRY LS1V3P,GAP
C00004 00003	α THE GAP FUNCTION - FOR WHICH ROOTS MUST BE FOUND
C00006 00004	α GET THE ANSWER BACK INTO THE ORIGINAL FRAME OF REFERANCE
C00009 00005	α LOCUS SOLUTION:  1 VIEW OF 3 POINTS CASE
C00012 00006	α FIND ZERO CROSSINGS
C00013 00007	α SETUP FOR ROOT FINDING ITERATION
C00014 ENDMK
C⊗;
ENTRY LS1V3P,GAP;
BEGIN "LS1V3P"
	REQUIRE "ABBREV[SYS,BGB]" SOURCE_FILE;
	REQUIRE "SAITRG[SYS,BGB]" SOURCE_FILE;

α IMPLICIT RESULTS;
	INTERNAL REAL D,WWW;
α COMPONENTS OF VIEWPOINT AND LANDMARK VECTORS;
	REAL X,Y,Z,XA,YA,ZA,XB,YB,ZB,XC,YC,ZC;
α COSINES OF RAYS TO THE 3 LANDMARK POINTS FROM THE VIEWPOINT;
	REAL COSα,COSβ,COSg;
α COMPONENTS OF LANDMARK TRIANGLE SIDE VECTORS;
	REAL DXA,DYA,DZA, DXB,DYB,DZB, DXC,DYC,DZC;
α THE LENGTH OF THE SIDES OF THE LANDMARK TRIANGLE;
	REAL A,B,C;
α IMPLICIT GAP VALUES;
	REAL XX,ZZ;
α INIT IMPLICIT GAP ARGUMENTS;
	REAL CTHETA,STHETA,COSPHI,SINPHI,TANPSI;
α COMPUTE THE ANGLES PHI AND PSI;

PROCEDURE PHIPSI (REAL Cα,Cβ,Cg);
BEGIN	"PHIPSI"
	REAL RAD,Sα,COSPSI,SINPSI,THETA,PHI,PSI;
	RAD	←	SQRT(Cg↑2 - 2*Cα*Cβ*Cg + Cβ↑2);
	Sα	←	SQRT(1-Cα↑2);
	COSPHI	←	Sα*Cβ/RAD;
	SINPHI	←	SQRT(1-COSPHI↑2);
	COSPSI	←	RAD/Sα;
	SINPSI	←	SQRT(1-COSPSI↑2);
	TANPSI	←	SINPSI/COSPSI;
	PHI	←	ACOS(COSPHI);
	THETA	←	ACOS(Cα) - ACOS(COSPHI);
	PSI	←	ACOS(COSPSI);
	STHETA	←	SIN(THETA);
	CTHETA	←	COS(THETA);
END	"PHIPSI";
α THE GAP FUNCTION - FOR WHICH ROOTS MUST BE FOUND;
	REAL WHI,WLO;
	REAL     R,S,H;
INTERNAL REAL PROCEDURE GAP (REAL W);
BEGIN	"GAP"
	REAL RSW,RCW,DX,DY,K;
	REAL XX1,DY1,XX2,DY2;

α L VECTOR;
	RSW	←	R*SIN(W);
	RCW	←	R*COS(W);
α (B-L) VECTOR WHEN W<0, (C-L) VECTOR WHEN W≥0;
	DX	←	-S-RCW;

α PHI ROTATION CCW OF (B-L) VECTOR;
	⊂ DY	←	+A/2-RSW;
	XX1	←	COSPHI*DX - SINPHI*DY;
	DY1	←	COSPHI*DY + SINPHI*DX;⊃;
α THETA ROTATION CW OF (C-L) VECTOR;
	⊂ DY	←	-A/2-RSW;
	XX2	←	CTHETA*DX + STHETA*DY;
	DY2	←	CTHETA*DY - STHETA*DX;⊃;
	IF W<0 THEN ⊂ DY←DY1;XX←XX1;⊃
	       ELSE ⊂ DY←DY2;XX←XX2;⊃;

α PSI ROTATION FOR Z COMPONENT;
	Z	←	SQRT(XX↑2+DY↑2)*TANPSI;
α SOLVE FOR XX & ZZ;
	K	←	(D-RSW)/DY;
	XX	←	RCW + K*XX;
	ZZ	←	K*Z;
	RETURN(SQRT((XX+S)↑2 + ZZ↑2)- H);
END	"GAP";
α GET THE ANSWER BACK INTO THE ORIGINAL FRAME OF REFERANCE;
PROCEDURE OFRAME;
BEGIN	"OFRAME"
	DEFINE	determinant (a11,a12,a13,a21,a22,a23,a31,a32,a33)=
	"(a11*(a22*a33-a23*a32)
	 -a12*(a21*a33-a23*a31)
	 +a13*(a21*a32-a22*a31))";
	REAL RCW,RSW,SXX,A2,RCWX,RSWD,PA2D,MA2D,II,JJ,KK,KA,KB,KC,K;
	REAL LA,LB,LC,G;
	IF ZZ<0 THEN OUTSTR("ZZ NEGATIVE."&13&10);
	RCW 	←	 R*COS(WWW);
	RSW 	←	 R*SIN(WWW);
α COMPUTE THE LENGTHS OF THE LEGS OF THE TRIPOD;
	LA  	←	 SQRT((RCW-XX)↑2 + (RSW-D)↑2 + ZZ↑2);
	LB  	←	 SQRT((RCW+S)↑2 + (RSW-A/2)↑2);
	LC  	←	 SQRT((RCW+S)↑2 + (RSW+A/2)↑2);
α COMPUTE SOME OF THE COMPONENTS OF THE LEG VECTORS,
  WITH RESPECT TO THE CHORD'S MIDPOINT FRAME OF REFERENCE;
	A2	←	A/2;
	SXX	←	-S-XX;
	RCWX	←	RCW-XX;
	RSWD	←	RSW-D;
	PA2D	←	A2-D;
	MA2D	←	-A2-D;
α VECTOR NORMAL TO THE PLANE OF THE LANDMARK TRIANGLE;
	II	←	DZB*DYC - DYB*DZC;
	JJ	←	DXB*DZC - DZB*DXC;
	KK	←	DYB*DXC - DXB*DYC;
α COMPUTE TRIPLE PRODUCT OF (C TO A) CROSS (C TO B) DOT (C TO L);
   KA ← -DXC*XA-DYC*YA-DZC*ZA+SXX*RCWX+PA2D*RSWD+ZZ↑2;
   KB ← DXB*XA+DYB*YA+DZB*ZA+SXX*RCWX+MA2D*RSWD+ZZ↑2;
   KC ← II*XA+JJ*YA+KK*ZA+A*ZZ*(RCW+S);
α APPLY CRAMER'S RULE;
	K	←	DETERMINANT(-DXC, -DYC, -DZC,
				     DXB,  DYB,  DZB,
			   	      II,   JJ,   KK);
	X	←	DETERMINANT(  KA, -DYC, -DZC,
				      KB,  DYB,  DZB,
			   	      KC,   JJ,   KK) / K;
	Y	←	DETERMINANT(-DXC,   KA, -DZC,
				     DXB,   KB,  DZB,
			   	      II,   KC,   KK) / K;
	Z	←	DETERMINANT(-DXC, -DYC,   KA,
				     DXB,  DYB,   KB,
			   	      II,   JJ,   KC) / K;
	IF ABS(K)≤0.01 THEN OUTSTR("KRAMER K WARNING "&CVG(K)&↓);
END	"OFRAME";
α LOCUS SOLUTION:  1 VIEW OF 3 POINTS CASE;
α LS1V3P RETURNS 
	-1 FOR NO SOLUTIONS GAP LOW, 
	 0 FOR NO SOLUTIONS GAP HIGH,
   OTHERWISE N FOR THAT MANY SOLUTIONS STASHED IN ARRAY V[1:N,1:3];
INTERNAL INTEGER PROCEDURE LS1V3P (REAL ARRAY V,P1,P2,P3,V3P);
BEGIN	"LS1V3P"
	INTEGER NROOTS;
α STASH ARGUMENT ARRAYS INTO WORKING VARIABLES;
	ARRBLT(XA,P1[1],3);
	ARRBLT(XB,P2[1],3);
	ARRBLT(XC,P3[1],3);
	ARRBLT(COSα,V3P[1],3);
α COMPUTE THE SIDES OF THE LANDMARK TRIANGLE;
	DXA ← XB - XC;	DYA ← YB - YC;	DZA ← ZB - ZC;
	DXB ← XC - XA;	DYB ← YC - YA;	DZB ← ZC - ZA;
	DXC ← XA - XB;	DYC ← YA - YB;	DZC ← ZA - ZB;
α COMPUTE LENGTHS OF THE SIDES OF THE LANDMARK TRIANGLE;
	A ← SQRT(DXA↑2 + DYA↑2 + DZA↑2);
	B ← SQRT(DXB↑2 + DYB↑2 + DZB↑2);
	C ← SQRT(DXC↑2 + DYC↑2 + DZC↑2);
α CHECK FOR COLINEAR CASE;
BEGIN
	REAL A,B,C,D;
	A	←	ACOS(COSα);
	B	←	ACOS(COSβ);
	C	←	ACOS(COSG);
	IF B>A THEN A↔B;
	IF C>A THEN A↔C;
	D	←	ABS(B+C-A);
	IF D≤0.005 THEN OUTSTR("COLINEAR WARNING	"&CVG(D)&13&10);
END;
BEGIN	"2ND BLK"
	REAL SINα,COSC,ALPHA,W,GLO,GHI,G;
α R IS THE RADIUS OF L'S CIRCLE;
α S IS THE DISTANCE TO THE AB CHORD FROM THE CENTER OF L'S CIRCLE;
	SINα	←	SQRT(1-COSα↑2);
	R	←	A/(2*SINα);
	S	←	R*COSα;
α H IS THE ALTITUDE OF THE LANDMARK TRIANGLE FROM C TO SIDE AB;
α H IS ALSO THE RADIUS OF THE SPINDLE;
	COSC	←	(A↑2 + B↑2 - C↑2)/(2*A*B);
	H	←	B*SQRT(1 - COSC↑2);
α D IS THE DIRECTED DISTANCE FROM THE FOOT OF THE ALTITUDE,
  TO THE MIDPOINT OF THE CHORD AB;
	D	←	B*COSC - A/2;
α FIND ZERO CROSSINGS;
BEGIN	"3RD BLK"
	REAL PROCEDURE ROOT (REAL WLO,WHI);
BEGIN	"ROOT"
	REAL GLO,GHI,W,G;
	GLO	←	GAP(WLO);
	GHI	←	GAP(WHI);
	DO
BEGIN	"HALVE INTERVAL" 
	W	←	(WLO + WHI)/2;
	IF ¬(WLO<W ∧ W<WHI) THEN DONE;
	G	←	GAP(W);
	IF G⊗GLO ≥ 0 THEN
BEGIN
	GLO	←	G;
	WLO	←	W;
END	ELSE
BEGIN
	GHI	←	G;
	WHI	←	W;
END;
END	"HALVE INTERVAL" UNTIL G=0 ∨ (WHI-WLO)<1/2↑25;
	RETURN(W);
END	"ROOT";
α SETUP FOR ROOT FINDING ITERATION;
	REAL DELTA,GOLD,G;
	ALPHA	←	ACOS(COSα);
	WLO 	←	ALPHA - π;
	WHI 	←	π - ALPHA;
	DELTA	←	(WHI-WLO)/200;
	PHIPSI(COSα,COSg,COSβ);
	G	←	GAP(WLO);
	NROOTS	←	0;
	FOR W←WLO STEP DELTA UNTIL WHI DO
BEGIN	"INTERVAL"
	GOLD	←	G;
	G	←	GAP(W);
	IF G*GOLD < 0  ∧ ABS(G-GOLD)<1 THEN
BEGIN	"ZERO CROSSING"
	WWW	←	ROOT(W-DELTA,W);
	NROOTS←NROOTS+1;
	OFRAME;
	V[NROOTS,1]←	X;
	V[NROOTS,2]←	Y;
	V[NROOTS,3]←	Z;
END	"ZERO CROSSING";
END	"INTERVAL";
	RETURN(IF NROOTS≠0 THEN NROOTS ELSE 
	       IF GOLD<0 THEN -1 ELSE 0);
END	"3RD BLK";
END	"2ND BLK";
END	"LS1V3P";

END	"LS1V3P";